#include <iostream>

#include "opencv2/opencv.hpp"

#include "apriltag_operate.h"


// 初始化apriltag, 设定apriltag各项参数
void apriltag::init(void)
{
    getopt = getopt_create();

    getopt_add_bool(getopt, 'h', "help", 0, "Show this help");
    getopt_add_bool(getopt, 'd', "debug", 0, "Enable debugging output (slow)");
    getopt_add_bool(getopt, 'q', "quiet", 0, "Reduce output");
    getopt_add_string(getopt, 'f', "family", "tag36h11", "Tag family to use");
    getopt_add_int(getopt, '\0', "border", "1", "Set tag family border size");
    getopt_add_int(getopt, 't', "threads", "1", "Use this many CPU threads");
    getopt_add_double(getopt, 'x', "decimate", "2.0", "Decimate input image by this factor"); // 2为最佳
    getopt_add_double(getopt, 'b', "blur", "0.8", "Apply low-pass blur to input");
    getopt_add_bool(getopt, '0', "refine-edges", 1, "Spend more time trying to align edges of tags");
    getopt_add_bool(getopt, '1', "refine-decode", 0, "Spend more time trying to decode tags");
    getopt_add_bool(getopt, '2', "refine-pose", 0, "Spend more time trying to precisely localize tags");

    // Initialize tag detector with options
    tf = NULL;
    famname = (char *)getopt_get_string(getopt, "family");
    if (!strcmp(famname, "tag36h11"))
        tf = tag36h11_create();
    else {
        printf("Unrecognized tag family name. Use e.g. \"tag36h11\".\n");
        exit(-1); //  fail
    }
    tf->black_border = getopt_get_int(getopt, "border");

    td = apriltag_detector_create();
    apriltag_detector_add_family(td, tf);                   // bit容错个数 设置
    td->quad_decimate = getopt_get_double(getopt, "decimate");
    td->quad_sigma = getopt_get_double(getopt, "blur");
    td->nthreads = getopt_get_int(getopt, "threads");
    td->debug = getopt_get_bool(getopt, "debug");
    td->refine_edges = getopt_get_bool(getopt, "refine-edges");
    td->refine_decode = getopt_get_bool(getopt, "refine-decode");
    td->refine_pose = getopt_get_bool(getopt, "refine-pose");

    count_defind_apriltag = -1; // 没有找到apriltag的次数，记满n次则开始全图搜索； 为-1时，代表运行后尚未找到apriltag
    whether_init_tracker = false; // tracker 是否重新设定roi区域
    num_apriltag = 0; // 检测到apriltag数量
}


// 清空动态分配的内存
void apriltag::destroy(void)
{

    std::cout << "begin destroy space" << std::endl;
    if(td != NULL)
    {
        apriltag_detector_destroy(td);
        td = NULL;
    }


    if(tf != NULL)
    {
        if (!strcmp(famname, "tag36h11"))
            tag36h11_destroy(tf);
        tf = NULL;
    }

    if(getopt != NULL)
    {
        getopt_destroy(getopt);
        getopt = NULL;
    }

}

// 找到靶标函数  全图
void apriltag::detect(cv::Mat &image_rgb)
{
    id = -1;

    if(image_rgb.empty())
    {
        std::cout << "image is empty in apriltga detect  " ;//<< std::endl;
        return;
    }

    if(image_rgb.channels() != 3)
    {
        std::cout << "image isn't rgb in apriltga detect  " ;//<< std::endl;
        return;
    }

    cv::Mat gray;
    cv::cvtColor(image_rgb, gray, cv::COLOR_BGR2GRAY); // 灰度化

    // Make an image_u8_t header for the Mat data
    image_u8_t im = { .width = gray.cols,
        .height = gray.rows,
        .stride = gray.cols,
        .buf = gray.data
    };

    zarray_t *detections = apriltag_detector_detect(td, &im);//找到apriltag

    num_apriltag = zarray_size(detections);
    //std::cout << num_apriltag << " tags detected  " ;//<< std::endl;

    if(num_apriltag >= 0)
    {
        for (int i = 0; i < num_apriltag; i++)
        {
            apriltag_detection_t *det;
            zarray_get(detections, i, &det);

            p[0][0] = det->p[0][0];
            p[0][1] = det->p[0][1];

            p[1][0] = det->p[1][0];
            p[1][1] = det->p[1][1];

            p[2][0] = det->p[2][0];
            p[2][1] = det->p[2][1];

            p[3][0] = det->p[3][0];
            p[3][1] = det->p[3][1];

            id = det->id;

            break;
        }
    }

    zarray_destroy(detections);

}


// 找到靶标函数   roi区域
void apriltag::detect(cv::Mat &image_rgb, cv::Rect2d &roi_fake)
{
    id = -1;

    if(roi_fake.x < 0 || roi_fake.y < 0 ||
       roi_fake.width <= 0 || roi_fake.height <= 0)
    {
        std::cout << "roi isn't legal in apriltga detect  " ;//<< std::endl;
        return;
    }

    if(image_rgb.empty())
    {
        std::cout << "image is empty in apriltga detect  " ;//<< std::endl;
        return;
    }

    if(image_rgb.channels() != 3)
    {
        std::cout << "image isn't rgb in apriltga detect  " ;//<< std::endl;
        return;
    }


    cv::Mat gray;
    cv::cvtColor(image_rgb(cv::Rect(roi_fake.x, roi_fake.y, roi_fake.width, roi_fake.height)), gray, cv::COLOR_BGR2GRAY);

    // Make an image_u8_t header for the Mat data
    image_u8_t im = { .width = gray.cols,
        .height = gray.rows,
        .stride = gray.cols,
        .buf = gray.data
    };

    zarray_t *detections = apriltag_detector_detect(td, &im);//找到apriltag

    num_apriltag = zarray_size(detections);
    //std::cout << num_apriltag << " tags detected  " ;//<< std::endl;

    if(num_apriltag >= 0)
    {
        for (int i = 0; i < num_apriltag; i++)
        {
            apriltag_detection_t *det;
            zarray_get(detections, i, &det);

            p[0][0] = det->p[0][0] + roi_fake.x;
            p[0][1] = det->p[0][1] + roi_fake.y;

            p[1][0] = det->p[1][0] + roi_fake.x;
            p[1][1] = det->p[1][1] + roi_fake.y;

            p[2][0] = det->p[2][0] + roi_fake.x;
            p[2][1] = det->p[2][1] + roi_fake.y;

            p[3][0] = det->p[3][0] + roi_fake.x;
            p[3][1] = det->p[3][1] + roi_fake.y;

            id = det->id;

            break;
        }
    }
    zarray_destroy(detections);
}

 // 勾勒出apriltag的外边
void apriltag::describe(cv::Mat &frame)
{
    if(id != -1)
    {
        cv::line(frame, cv::Point(p[0][0], p[0][1]),
                    cv::Point(p[1][0], p[1][1]),
                    cv::Scalar(0, 0xff, 0), 2);
        cv::line(frame, cv::Point(p[0][0], p[0][1]),
                    cv::Point(p[3][0], p[3][1]),
                    cv::Scalar(0, 0, 0xff), 2);
        cv::line(frame, cv::Point(p[1][0], p[1][1]),
                    cv::Point(p[2][0], p[2][1]),
                    cv::Scalar(0xff, 0, 0), 2);
        cv::line(frame, cv::Point(p[2][0], p[2][1]),
                    cv::Point(p[3][0], p[3][1]),
                    cv::Scalar(0xff, 0, 0), 2);
    }
    else
        std::cout << "can't describe the apriltag" << std::endl;
}

// 使roi包括整幅图
void apriltag::roi_restore(cv::Mat &frame)
{
    if(frame.empty())
    {
        std::cout << "image is empty in roi restore" << std::endl;
        return;
    }
    roi.x = 0;
    roi.y = 0;
    roi.width = frame.cols - roi.x - 1;
    roi.height = frame.rows - roi.y - 1;
}

// 让roi不超出图像
void apriltag::roi_correct(cv::Mat &frame)
{
    if(frame.empty())
    {
        std::cout << "image is empty in roi correct" << std::endl;
        return;
    }
    if(roi.x < 0) roi.x = 0;
    if(roi.y < 0) roi.y = 0;
    if(roi.x + roi.width > frame.cols) roi.width = frame.cols - roi.x - 1;
    if(roi.y + roi.height > frame.rows) roi.height = frame.rows - roi.y - 1;
    if(roi.width < 0) roi.width = 0;
    if(roi.height < 0) roi.height = 0;
}

// 根据apriltag的四个角，得到roi
void apriltag::roi_get(double magnify_rate)
{
    if(magnify_rate < 0)
    {
        std::cout << "magnify rate < 0" << std::endl;
        return;
    }

    double x_max = p[0][0],
           x_min = p[0][0],
           y_max = p[0][1],
           y_min = p[0][1];

     for(int i = 1; i < 4; i ++)
     {
        if(x_max < p[i][0]) x_max = p[i][0];
        if(x_min > p[i][0]) x_min = p[i][0];
        if(y_max < p[i][1]) y_max = p[i][1];
        if(y_min > p[i][1]) y_min = p[i][1];
     }

    roi.width = (x_max - x_min) * magnify_rate;
    roi.height = (y_max - y_min) * magnify_rate;

    roi.x = x_min - roi.width / magnify_rate * (magnify_rate - 1)/2;
    roi.y = y_min - roi.height / magnify_rate * (magnify_rate - 1)/2;
}

// 放大/缩小　roi
void apriltag::roi_magnify(double magnify_rate)
{
    roi.x = roi.x - roi.width * (magnify_rate - 1)/2;
    roi.y = roi.y - roi.height * (magnify_rate - 1)/2;

    roi.width = roi.width * magnify_rate;
    roi.height = roi.height * magnify_rate;
}

